﻿
#include <iostream>
#include <Windows.h>

#include "AnalisysImage.h"
#include "ImageEvaluate.h" 
//#include "OffsetHelp.h"
#include "pubHead.h"
//#include "QImgType.h"
#include <vector>
//#ifdef NDEBUG
//#pragma comment(lib, "../library64/opencv_world300.lib")
//#else
//#pragma comment(lib, "../library64/opencv_world300d.lib")
//#endif

//////先定义暂时解决编译问题，后续细化////////////////
#define BITS_16 1
#define BITS_8 0
struct QImgType
{
	static const int mImgBits = BITS_8;
};

namespace analysis_img
{
	using namespace std;

	bool testInts(int a[])
	{
		//size a  = 5; a0<a1<a2>a3>a4 && 
		if (a[1] > a[3])
		{
			//if (a[0] + a[1] < a[3] + a[4]) return false;
			if (a[0] > a[3] || a[4] > a[0]) return false;
		}
		else
		{
			if (a[1] < a[4] || a[4] < a[0]) return false;
		}
		return true;
	}
	bool goodPeakJudge(int x, int y, cv::Mat &img)
	{
		int count = 0;
		unsigned short centerVal = img.at<unsigned short>(x, y);

		if (img.at<unsigned short>(x - 1, y - 1)< img.at<unsigned short>(x - 2, y - 2)) count++;
		if (img.at<unsigned short>(x + 1, y + 1) < img.at<unsigned short>(x + 2, y + 2)) count++;
		if (img.at<unsigned short>(x + 1, y - 1) < img.at<unsigned short>(x + 2, y - 2)) count++;
		if (img.at<unsigned short>(x - 1, y + 1) < img.at<unsigned short>(x - 2, y + 2)) count++;

		if (img.at<unsigned short>(x - 1, y) < img.at<unsigned short>(x - 2, y)) count++;
		if (img.at<unsigned short>(x + 1, y)< img.at<unsigned short>(x + 2, y)) count++;
		if (img.at<unsigned short>(x, y - 1) < img.at<unsigned short>(x, y - 2)) count++;
		if (img.at<unsigned short>(x, y + 1) < img.at<unsigned short>(x, y + 2)) count++;

		int a[5] = { 0 };
		a[0] = img.at<unsigned short>(x - 2, y);
		a[1] = img.at<unsigned short>(x - 1, y);
		a[2] = centerVal;
		a[3] = img.at<unsigned short>(x + 1, y);
		a[4] = img.at<unsigned short>(x + 2, y);
		if (!testInts(a)) count++;
		a[0] = img.at<unsigned short>(x, y - 2);
		a[1] = img.at<unsigned short>(x, y - 1);
		a[3] = img.at<unsigned short>(x, y + 1);
		a[4] = img.at<unsigned short>(x, y + 2);
		if (!testInts(a)) count++;
		//return count;
		if (count < 3)
			return true;
		else
			return false;
	}

	float getScore(int x, int y, const cv::Mat &img, int t = 1)
	{
        int EightPixelInts = img.at<unsigned short>(x - t, y) +
            img.at<unsigned short>(x - t, y - t) + img.at<unsigned short>(x - t, y + t) +
            img.at<unsigned short>(x + t, y) + img.at<unsigned short>(x + t, y - t) +
            img.at<unsigned short>(x + t, y + t) + img.at<unsigned short>(x, y - t) +
            img.at<unsigned short>(x, y + t);
		int centerPixelInts = img.at<unsigned short>(x, y);
		float meanPeakInts = 1.0*(EightPixelInts + centerPixelInts) / 9;
		//std::cout << meanPeakInts << " ";
		float score = 1.0*(8 * centerPixelInts - EightPixelInts) / meanPeakInts;
		return score;
	}
	float scoreFunc(int x, int y, const cv::Mat &img, float &X, float &Y)
	{
		int v[9] = { 0 };
		int index = 0;
		for (auto i = (x - 1); i < (x + 2); ++i)
		{
			for (auto j = (y - 1); j < (y + 2); ++j)
			{
				v[index] = img.at<unsigned short>(i, j);
				index++;
			}
		}

		int sum = v[0] + v[1] + v[2] + v[3] + v[4] + v[5] + v[6] + v[7] + v[8];
		X = x + 1.0 * (v[6] + v[7] + v[8] - v[0] - v[1] - v[2]) / sum;
		Y = y + 1.0 * (v[2] + v[5] + v[8] - v[0] - v[3] - v[6]) / sum;

		auto resultFun = [=](int x, int y, const cv::Mat& img, int t)
			{
				int EightPixelInts = img.at<unsigned short>(x - t, y) +
					img.at<unsigned short>(x - t, y - t) + img.at<unsigned short>(x - t, y + t) +
					img.at<unsigned short>(x + t, y) + img.at<unsigned short>(x + t, y - t) +
					img.at<unsigned short>(x + t, y + t) + img.at<unsigned short>(x, y - t) +
					img.at<unsigned short>(x, y + t);
				int centerPixelInts = img.at<unsigned short>(x, y);
				float meanPeakInts = 1.0 * (EightPixelInts + centerPixelInts) / 9;
				float score = 1.0 * (8 * centerPixelInts - EightPixelInts) / meanPeakInts;
				return score;
			};

		return resultFun(x, y, img, 3);
	}

	bool basicPeak(int x, int y, const cv::Mat &img, int threshold)
	{
		int center = img.at<unsigned short>(x, y);
		int pixel3_3[9] = { img.at<unsigned short>(x - 1, y - 1),
			img.at<unsigned short>(x - 1, y),
			img.at<unsigned short>(x - 1, y + 1),
			img.at<unsigned short>(x, y - 1),
			center,
			img.at<unsigned short>(x, y + 1),
			img.at<unsigned short>(x + 1, y),
			img.at<unsigned short>(x + 1, y - 1),
			img.at<unsigned short>(x + 1, y + 1)
		};
		for (int i = 0; i < 4; i++)
		{
			if (center <= pixel3_3[i]) return false;
			if (threshold > pixel3_3[i]) return false;
		}
		for (int i = 5; i < 9; i++)
		{
			if (center <= pixel3_3[i]) return false;
			if (threshold > pixel3_3[i]) return false;
		}
		return true;
	}

	static inline float parabola_extremum(float xi[], float yi[])
	{
		float numerator = 0.;
		float denominator = 0.;
		register short i;
		for (i = 0; i < 3; i++) {
			int i2 = (i + 1) % 3;
			int i3 = (i + 2) % 3;
			numerator += yi[i] * (xi[i2] * xi[i2] - xi[i3] * xi[i3]);
			denominator += yi[i] * (xi[i2] - xi[i3]);
		}
		if (fabs(denominator) > 1.e-14)
			return numerator / (2.f*denominator);
		return xi[1];
	}

	static inline float localSNR(cv::Mat &image, cv::Mat &signal, int x, int y, int win){
		int dx = x - win > 0 ? x - win : 0;
		int ux = x + win < image.rows ? x + win : image.rows - 1;
		int dy = y - win > 0 ? y - win : 0;
		int uy = y + win < image.cols ? y + win : image.cols - 1;

		double mean = 0, cov = 0;
		double max = 0;
		float number = 1e-10, num = 1e-10;

		for (int i = dx; i <= ux; i++){
			for (int j = dy; j <= uy; j++){
				max += image.at<ushort>(i, j);
				num++;
			}
		}
		max /= num;

		for (int i = dx; i <= ux; i++){
			for (int j = dy; j <= uy; j++){
				if (image.at<ushort>(i, j) <= max){
					number++;
					mean += image.at<ushort>(i, j);
				}
			}
		}
		mean /= number;

		for (int i = dx; i <= ux; i++){
			for (int j = dy; j <= uy; j++){
				if (image.at<ushort>(i, j) <= max){
					cov += (image.at<ushort>(i, j) - mean) * (image.at<ushort>(i, j) - mean);
				}
			}
		}

		cov = std::sqrt(cov / number);

		if (number < 1 || num < 1 || signal.at<float>(x, y) <= 0 || cov == 0){
			return 0;
		}
		float xx = signal.at<float>(x, y);
		return 10 * std::log(signal.at<float>(x, y) / cov) / std::log(10);

	}


	int peakLocation::run(cv::Mat &image, std::vector<cv::Point2f> &peaks_center, cv::Mat &bim, std::vector<float>& pscore, const char *pData, float &snr, float &ith, float &pavg, float &pvar, float &psc, float &bgv, float &bgvar, float &pbr){

		if (image.rows == 0 || image.cols == 0)
			return -1;

		imageFilter_AnaImg imf;
		unsigned long n = 0;
		int goodPeakCount = 0;
		int64 start, end;

		start = cv::getTickCount();

		cv::Mat imageF, noise, bg, mse, imageB;
		imageB = image.clone();
		imageB.convertTo(imageF, CV_32F);
		imf.impoccess(imageF, bg, mse, noise, gsegma, bim, n);
		bim = bg.clone(); 
		int sth = STH;
		imf.OtsuThreshold(imageF, sth);

		bgv = cv::mean(bg)[0];
		gauss(gsegma);
        cv::Mat result;
        noise.convertTo(result, CV_8UC1);
        //cv::imwrite("noise.tif", result*255);
		fastLaunch(noise, imageF, bg, noisecounter, sth, mse, peaks_center, pscore, bgv * JGGL); //could for low Intensity


		end = cv::getTickCount();
		if (peaks_center.size() > 100000){
			std::cout << "Warning : Too Many Peaks" << std::endl;
		}

		//QC
		cv::Mat t1, t2;
		cv::meanStdDev(bg, t1, t2);
		bgvar = t2.at<double>(0, 0);
		ith = sth;
		double ssnr = 0;
		double sum = 0;
		std::vector< float > inten;
		cv::Mat m_tempOut = cv::Mat(1, 1, CV_32F);
		cv::Mat image32F;
		image.convertTo(image32F, CV_32F);
		size_t pos = 0;
		std::vector<float> snrvt;
		for (pos = 0; pos < peaks_center.size(); pos++){
			cv::getRectSubPix(image32F, cv::Size(1, 1), peaks_center[pos], m_tempOut); //image instead of imageF
			inten.push_back(m_tempOut.at<float>(0, 0));
			sum += inten[pos];
			snrvt.push_back(localSNR(image, mse, std::round(peaks_center[pos].y), std::round(peaks_center[pos].x), 9)); //////////////////////cost time
			ssnr += snrvt[pos];
		}
		if (peaks_center.size() != 0){
			pavg = sum / peaks_center.size();
			snr = ssnr / peaks_center.size();
			for (pos = 0; pos < peaks_center.size(); pos++){
				pscore[pos] /= pavg;
				if (pscore[pos]>PFT)
					goodPeakCount++;
			}
		}
		sum = 0;

		int win = 9;
		double *x = new double[win*win];
		double *y = new double[win*win];
		double *z = new double[win*win];
		for (int i = 0; i < win*win; i++) z[i] = 0;
		double t[] = { 0, 0, 0, 0, 0, 0 };
		int x0 = std::round(win / 2.0) - 1;
		int y0 = std::round(win / 2.0) - 1;
		if (peaks_center.size() != 0){
			for (short i = 0, k = 0; i < win; i++){
				for (short j = 0; j < win; j++){
					x[k] = i;
					y[k] = j;
					k++;
				}
			}
			for (pos = 0; pos < peaks_center.size(); pos++){
				int xx = std::round(peaks_center[pos].y), yy = std::round(peaks_center[pos].x);
				if (win < xx && win < yy && xx < image.rows - win && yy < image.cols - win){
					for (short i = xx - x0, k = 0; i <= xx + x0; i++){
						for (short j = yy - y0; j <= yy + y0; j++){
							z[k] += mse.at<float>(i, j) / mse.at<float>(xx, yy);
							k++;
						}
					}
				}
			}
			xwb::gaussian2DFitn(x, y, z, win*win, t);
		}
		delete[]x;
		delete[]y;
		delete[]z;
		if (peaks_center.size() != 0)
		{
			for (pos = 0; pos < inten.size(); pos++){
				sum += (inten[pos] - pavg) * (inten[pos] - pavg);
			}
			pvar = std::sqrt(sum / peaks_center.size());
			pbr = n * 1.0 / peaks_center.size();
			psc = (t[2] + t[3]) / 2 * 2.35482;
			if (!std::isfinite(psc))
				psc = 0;
		}
		//QC
		if (inten.size() != pscore.size())
			std::cout << "error inten size" << inten.size() << " pscore: " << pscore.size() << std::endl;
		pscore.assign(inten.begin(), inten.end());
		return goodPeakCount;
	}

	void peakLocation::fastLaunch(cv::Mat &noise, const cv::Mat &intensity, cv::Mat &bg, float noisecounter, float sth, cv::Mat& mse, std::vector<cv::Point2f> &resultPosition, std::vector<float> &psc, float jg)
	{
		cv::Point2f pos;
		for (int i = 3; i < noise.rows - 3; ++i)
		{
			for (int j = 3; j < noise.cols - 3; ++j)
			{
				if (noise.at<float>(i, j) != 0)
				{
					bool ispeak = true;

					int bi = i - 1;
					int ei = i + 1;
					int bj = j - 1;
					int ej = j + 1;

					int counter = 0;
					int num = 0;
					int ln = 0;
					for (int k = bi; k <= ei; k++){
						if (!ispeak) break;
						for (int l = bj; l <= ej; l++){
							num++;
							if (noise.at<float>(k, l) != 0){
								counter++;
							}
                            if (intensity.at<float>(i, j) < intensity.at<float>(k, l)){
								ispeak = false;
								break;
							}
						}
					}



					float x0 = std::round(winw / 2.0) - 1;
					float y0 = std::round(winw / 2.0) - 1;
					float gvv = 0;
					for (register short k1 = 0; k1 < winw; k1++){
						for (register short k2 = 0; k2 < winw; k2++){
							int xp = i + x0 - k1, yp = j + y0 - k2;
							gvv += tpl[k1][k2] * intensity.at<float>(xp, yp);
							dt[k1][k2] = intensity.at<float>(xp, yp);
						}
					}
					gvv /= gjt;

					double gv = gaussMatch(dt);


					if (ispeak && counter > noisecounter * num && gv * gvv * counter / num > sth && gvv > jg){

						psc.push_back(gvv * gv);

						float sumx = 0;
						float sumy = 0;
						float sum = 0;
						float xv[3], yv[3];

						for (register short h = -1; h <= 1; h++) {
							xv[h + 1] = h;
							yv[h + 1] = intensity.at<float>(i + h, j);
						}
						pos.y = parabola_extremum(xv, yv) + i;

						for (register short h = -1; h <= 1; h++) {
							xv[h + 1] = h;
							yv[h + 1] = intensity.at<float>(i, j + h);
						}
						pos.x = parabola_extremum(xv, yv) + j;

						//pos error:NaN , out of range
						resultPosition.push_back(pos);

					}
				}
			}
		}

	}

	double peakLocation::gaussMatch(float **a){
		double fz = 0;
		double fm1 = 0;
		double fm2 = 0;
		double xj = 0;
		int count = 0;
		for (register short i = 0; i < winw; i++){
			for (register short j = 0; j < winw; j++){
				xj += a[i][j];
				count++;
			}
		}
		xj /= count;
		for (register short i = 0; i < winw; i++){
			for (register short j = 0; j < winw; j++){
				fz += (a[i][j] - xj) * (tpl[i][j] - gj);
				fm1 += (a[i][j] - xj) * (a[i][j] - xj);
				fm2 += (tpl[i][j] - gj) * (tpl[i][j] - gj);
			}
		}
		if (fm1 == 0 || fm2 == 0) return 0;
		return  fz / std::sqrt(fm1*fm2);
	}

	void peakLocation::gauss(float a0){
		gj = 0;
		gjt = 0;
		float x0 = std::round(winw / 2.0) - 1;
		float y0 = std::round(winw / 2.0) - 1;
		for (register short i = 0; i < winw; i++){
			for (register short j = 0; j < winw; j++){
				tpl[i][j] = 1.0 / (std::sqrt(2 * PI) * a0) * exp(-((i - y0) * (i - y0) + (j - x0) * (j - x0)) / 2.0 / (a0 * a0));
				gj += tpl[i][j];
			}
		}
		gjt = gj;
		gj /= (winw * winw);
	}
};

